#!/usr/bin/env python3
# 完整的PCA9685水平舵机测试程序

import smbus
import time
import logging

# 设置日志
logging.basicConfig(
    level=logging.INFO,
    format='%(asctime)s - %(name)s - %(levelname)s - %(message)s'
)
logger = logging.getLogger('ServoTest')


class PCA9685:
    """PCA9685 PWM控制器类"""
    # 寄存器地址
    __SUBADR1 = 0x02
    __SUBADR2 = 0x03
    __SUBADR3 = 0x04
    __MODE1 = 0x00
    __PRESCALE = 0xFE
    __LED0_ON_L = 0x06
    __LED0_ON_H = 0x07
    __LED0_OFF_L = 0x08
    __LED0_OFF_H = 0x09

    def __init__(self, address=0x40, debug=False):
        self.bus = smbus.SMBus(1)  # 使用I2C总线1
        self.address = address
        self.debug = debug
        self.setup_pwm()

    def setup_pwm(self):
        """初始化PWM设置"""
        self.write(self.__MODE1, 0x00)  # 复位
        self.set_pwm_freq(50)  # 设置50Hz频率（标准舵机频率）
        logger.info("PCA9685初始化完成，PWM频率设置为50Hz")

    def write(self, reg, value):
        """写入I2C寄存器"""
        self.bus.write_byte_data(self.address, reg, value)
        if self.debug:
            logger.debug(f"I2C写入: 寄存器0x{reg:02X} <- 0x{value:02X}")

    def read(self, reg):
        """读取I2C寄存器"""
        result = self.bus.read_byte_data(self.address, reg)
        if self.debug:
            logger.debug(f"I2C读取: 寄存器0x{reg:02X} -> 0x{result:02X}")
        return result

    def set_pwm_freq(self, freq):
        """设置PWM频率"""
        prescaleval = 25000000.0  # 25MHz
        prescaleval /= 4096.0  # 12-bit
        prescaleval /= float(freq)
        prescaleval -= 1.0
        prescale = int(prescaleval + 0.5)

        oldmode = self.read(self.__MODE1)
        newmode = (oldmode & 0x7F) | 0x10  # 进入睡眠模式
        self.write(self.__MODE1, newmode)
        self.write(self.__PRESCALE, prescale)
        self.write(self.__MODE1, oldmode)
        time.sleep(0.005)
        self.write(self.__MODE1, oldmode | 0x80)

    def set_pwm(self, channel, on, off):
        """设置单个PWM通道"""
        self.write(self.__LED0_ON_L + 4 * channel, on & 0xFF)
        self.write(self.__LED0_ON_H + 4 * channel, on >> 8)
        self.write(self.__LED0_OFF_L + 4 * channel, off & 0xFF)
        self.write(self.__LED0_OFF_H + 4 * channel, off >> 8)
        if self.debug:
            logger.debug(f"设置PWM通道{channel}: ON={on}, OFF={off}")

    def set_servo_angle(self, channel, angle):
        """设置舵机角度(0-180度)"""
        if angle < 0:
            angle = 0
        elif angle > 180:
            angle = 180

        # 将角度转换为PWM脉冲宽度 (500-2500μs)
        pulse_width = 500 + (angle / 180.0) * 2000
        pulse = int(pulse_width * 4096 / 20000)  # 转换为12-bit值

        self.set_pwm(channel, 0, pulse)
        logger.info(f"通道{channel}舵机设置为{angle}度 (PWM: {pulse})")


def run_servo_test():
    """执行舵机测试程序"""
    print("=== PCA9685 水平舵机测试程序 ===")
    print("确保:")
    print("1. PCA9685已正确连接")
    print("2. 舵机信号线连接到指定通道")
    print("3. 电源供应充足")
    print("4. 机械结构可以自由移动")
    print("按Ctrl+C可随时终止测试\n")

    # 初始化PCA9685
    try:
        pca = PCA9685(debug=True)
    except Exception as e:
        logger.error(f"初始化PCA9685失败: {str(e)}")
        return

    # 配置测试参数
    SERVO_CHANNEL = 9  # 水平舵机通道号
    TEST_CYCLES = 2  # 测试循环次数
    MIN_ANGLE = 20  # 最小角度(防止机械碰撞)
    MAX_ANGLE = 160  # 最大角度
    CENTER_ANGLE = 90  # 中心位置

    try:
        # 测试1: 基本功能测试
        logger.info("=== 开始基本功能测试 ===")
        pca.set_servo_angle(SERVO_CHANNEL, CENTER_ANGLE)
        time.sleep(1)

        # 测试2: 极限位置测试
        logger.info("=== 极限位置测试 ===")
        for angle in [MIN_ANGLE, MAX_ANGLE, CENTER_ANGLE]:
            pca.set_servo_angle(SERVO_CHANNEL, angle)
            time.sleep(1)

        # 测试3: 扫描测试
        logger.info("=== 扫描测试 ===")
        for _ in range(TEST_CYCLES):
            # 正向扫描
            for angle in range(MIN_ANGLE, MAX_ANGLE + 1, 5):
                pca.set_servo_angle(SERVO_CHANNEL, angle)
                time.sleep(0.05)

            # 反向扫描
            for angle in range(MAX_ANGLE, MIN_ANGLE - 1, -5):
                pca.set_servo_angle(SERVO_CHANNEL, angle)
                time.sleep(0.05)

        # 测试4: 快速响应测试
        logger.info("=== 快速响应测试 ===")
        test_angles = [CENTER_ANGLE, 60, 120, CENTER_ANGLE]
        for _ in range(TEST_CYCLES * 2):
            for angle in test_angles:
                pca.set_servo_angle(SERVO_CHANNEL, angle)
                time.sleep(0.3)

        # 返回中心位置
        pca.set_servo_angle(SERVO_CHANNEL, CENTER_ANGLE)
        logger.info("=== 测试完成 ===")

    except KeyboardInterrupt:
        logger.info("测试被用户中断")
    except Exception as e:
        logger.error(f"测试过程中发生错误: {str(e)}")
    finally:
        # 确保舵机回到中心位置
        pca.set_servo_angle(SERVO_CHANNEL, CENTER_ANGLE)
        time.sleep(0.5)

        # 关闭所有PWM输出
        for ch in range(16):
            pca.set_pwm(ch, 0, 0)
        logger.info("已关闭所有PWM输出")


if __name__ == "__main__":
    run_servo_test()